/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef TGIMI_PROTOCOL_AVANT_CMD_V3_H
#define TGIMI_PROTOCOL_AVANT_CMD_V3_H
#include "crc16.h"
#include "owndebug.h"
#include "TGIMI_PROTOCOL_AVANT_CMD_V3_1.h"
#define TGIMI_PROTOCOL_AVANT_CMD_V3_SIZE         (8)

struct TGIMI_PROTOCOL_AVANT_CMD_V3
{
  enum EGear { gearForward=1, gearFree=0, gearBackward=2 };
  enum ETelescope { telescopeOut=2, telescopeStop=0, telescopeIn=1 };
      
public:
  TGIMI_PROTOCOL_AVANT_CMD_V3()
    : throttle(-127),
      wheel(0),
      joystickY(0),
      joystickX(0),
      externalHydr(0),
      digitalGear(0),
      digitalDoublePump(0),
      digitalLights(0),
      digitalBreaks(1),
      digitalTelescope(0),
      digitalRemoteCtrl(1),
      index(0),
      crc_low(0) {}
  
  ~TGIMI_PROTOCOL_AVANT_CMD_V3() {}

  TGIMI_PROTOCOL_AVANT_CMD_V3&operator=(const TGIMI_PROTOCOL_AVANT_CMD_V3_1 &other){
    throttle= other.throttle;      
    wheel= other.wheel;
    joystickY = other.joystickY;
    joystickX = other.joystickX; 
    externalHydr = other.externalHydr;    
    
    digitalGear = other.digitalGear;  
    digitalDoublePump = other.digitalDoublePump;
    digitalLights = other.digitalLights   ;
    digitalBreaks = other.digitalBreaks;
    digitalTelescope = other.digitalTelescope;
    digitalRemoteCtrl = other.digitalRemoteCtrl;
    index = other.index;
    crc_low = other.crc_low;
    return *this;
  }
  static unsigned short CreateCRC(unsigned char *data, int uLen)
  { 
    return crc16((const char *)data,(int)uLen);
  }
  
  void Print(const int aLvl) const
  {
    dPrint(aLvl,
	   "TeleProtocol_v3:\r\n"
	   "\tThrottle: %d\r\n"
	   "\tSteering: %d\r\n"
	   "\tJoyX: %d\r\n"
	   "\tJoyY: %d\r\n"
	   "\tExt: %d\r\n"
	   "\tRemote: %d, TeleS: %d, Breaks: %d, Light: %d, Pumps: %d, Gear: %d\r\n"
	   "\tIndex: %d\r\n"
	   "\tCRC: %x",
	   throttle,
	   wheel,
	   joystickX,
	   joystickY,
	   externalHydr,
	   digitalRemoteCtrl,
	   digitalTelescope,
	   digitalBreaks,
	   digitalLights,
	   digitalDoublePump,
	   digitalGear,
	   index,
	   crc_low);
  }
  
  void SetThrottle(float aPercentValue)
  {
    const int value = (const int)((aPercentValue * 254.0) - 127.0);
    
    if (aPercentValue >= 0.00 && aPercentValue <= 1.00) {
      this->throttle = value;
      
      dPrint(10, "Throttle set to: %d (%d = %.2f%%)",
	     this->throttle, value, 100.0*aPercentValue);
      
    } else if (aPercentValue < 0.00) {
      this->throttle = -127;
      
      dPrint(10, "Throttle set to: %d (%d = %.2f%%)",
	     this->throttle, value, 100.0*aPercentValue);
      
      
    } else {
      dPrint(3,"Throttle value out-of-range. (%f)", aPercentValue);
      
    }
  }
  
  void SetWheel(float aPercentValue)
  {
    if (aPercentValue >= -1.00 && aPercentValue <= 1.00) {
      const int value = (const int)((aPercentValue * 127.0));
      this->wheel = value;
      
      dPrint(10,"Wheel set to: %d (%d = %.2f%%)",
	     this->wheel, value, 100.0*aPercentValue);
    } else {
      dPrint(3,"Wheel value out-of-range. (%f)", aPercentValue);
      
    }
  }

  void SetJoyX(float aPercentValue)
  {
    if (aPercentValue < -1.00) aPercentValue = -1.00;
    if (aPercentValue > 1.00) aPercentValue = 1.00;
    
    const int value = (const int)(aPercentValue * 127.0);
    this->joystickX = value;
    
    dPrint(10,"JoyX set to: %d (%d = %.2f%%)",
	   this->joystickX, value, 100.0*aPercentValue);
  }

  void SetJoyY(float aPercentValue)
  {
    if (aPercentValue < -1.00) aPercentValue = -1.00;
    if (aPercentValue > 1.00) aPercentValue = 1.00;
    
    const int value = (const int)(aPercentValue * 127.0);
    this->joystickY = value;
    
    dPrint(10,"JoyY set to: %d (%d = %.2f%%)",
	   this->joystickY, value, 100.0*aPercentValue);
  }
  
  void SetExternalHydraulics(float aPercentValue)
  {
    if (aPercentValue < -1.00) aPercentValue = -1.00;
    if (aPercentValue > 1.00) aPercentValue = 1.00;
    
    const int value = (const int)(aPercentValue * 127.0);
    this->externalHydr = value;
    
    dPrint(10,"ExternalHydr set to: %d (%d = %.2f%%)",
	   this->externalHydr, value, 100.0*aPercentValue);
  }
    
  void SetGear(EGear aGear)
  {
    this->digitalGear = aGear;
  }

  void SetDoublePump(bool aEnabled)
  {
    this->digitalDoublePump = aEnabled;
  }

  void SetLights(bool aEnabled)
  {
    this->digitalLights = aEnabled;
  }
  
  void SetBreaks(bool aEnabled)
  {
    this->digitalBreaks = aEnabled;
  }

  void SetTelescope(ETelescope aTelescope)
  {
    this->digitalTelescope = aTelescope;
  }

  void SetRemote(bool aEnabled)
  {
    this->digitalRemoteCtrl = aEnabled;
  }

  void SetIndex(int aIndex)
  {
    this->index = aIndex;
  }

  void IncrementIndex(void)
  {
    this->index ++;
  }

  void CalculateCRC(void)
  {
    unsigned short int crc = CreateCRC((unsigned char *)this,
				       sizeof(TGIMI_PROTOCOL_AVANT_CMD_V3)-1);
    crc_low = crc & 0xFF;
   
  }

public:
  char throttle;        ///< Throttle setting (-127 up, 127 down)
  char wheel;           ///< Wheel setting (-127 left, 127 right)    
  char joystickY;       ///< 'Puomi' (-127 max speed up, 127 max speed down, 0 stop)
  char joystickX;       ///< 'Kauha' (-127 max up, 127 max down, 0 stop)
  char externalHydr;    ///< 'External Hydraulics' (-127 neg, 127 pos, 0 closed)
  
  unsigned digitalGear        : 2; ///< Gear; 0:free, 1:fw, 2:bw
  unsigned digitalDoublePump  : 1; ///< Two pumps enabled, otherwise one
  unsigned digitalLights      : 1; ///< Light on 
  unsigned digitalBreaks      : 1; ///< Breaks on (1)
  unsigned digitalTelescope   : 2; ///< Telescope; 0:stop, 1:in, 2:out
  unsigned digitalRemoteCtrl  : 1; ///< Remote control?
  
  
  char index;            ///< Packet index, from -127 -> 127 (Use nextIndex())
  unsigned char crc_low;          ///< Lower byte of CCIT CRC16 (use calculateCRC())
  
};
#endif
